#include "chipmuck-stub.h"
#include "chipmuck-simconn.h"
#include "hw/hw.h"
#include "sysemu.h"
#include "cpu-all.h"
#include "../errno.h"

int chipmuck_event_id = 0;
CPUState *chipmuck_env = 0;
uint32_t chipmuck_access_addr = 0;

int chipmuck_index = 1;
int chipmuck_bp_num = 0, chipmuck_wp_num = 0;
struct chipmuck_breakpoint chipmuck_bps[MAX_BP_NUM];
struct chipmuck_watchpoint chipmuck_wps[MAX_BP_NUM];

int chipmuck_set_breakpoint(int type, void *data, int size, int *index);
int chipmuck_del_breakpoint(int index);
int chipmuck_dump_data(unsigned int addr, void *data, int size);
int chipmuck_get_reg(unsigned int *regs, int *nreg);

void chipmuck_stub_init() {
	struct simconn_callback callback;
	
	// simconn initialization
	simconn_connect();
	callback.set_breakpoint = chipmuck_set_breakpoint;
	callback.dump_data = chipmuck_dump_data;
	callback.del_breakpoint = chipmuck_del_breakpoint;
	simconn_set_callback(&callback);
	
	// Add breakpoint handler
	qemu_add_vm_stop_handler(chipmuck_stub_handler, 0);
}

void chipmuck_stub_handler(void *data, int reason) {
	int i;
	
	// Break to simconn
	switch (chipmuck_event_id) {
	case EVENT_ID_BREAKPOINT:
		for (i = 0; i < chipmuck_bp_num; i++)
			if (chipmuck_env->eip == chipmuck_bps[i].addr)
				simconn_breakpoint(chipmuck_bps[i].index);
		break;
	case EVENT_ID_WATCHPOINT:
		for (i = 0; i < chipmuck_wp_num; i++)
			if (chipmuck_access_addr == chipmuck_wps[i].addr)
				simconn_breakpoint(chipmuck_wps[i].index);
		break;
	case EVENT_ID_TIMER_INT:
		simconn_breakpoint(-1);
		break;
	}
	
	// Start machine
	chipmuck_event_id = EVENT_ID_BREAKPOINT;
	vm_start();
}

int chipmuck_del_breakpoint(int index) {
	int i;
	CPUState *env;

	for (i = 0; i < chipmuck_bp_num; i++)
		if (chipmuck_bps[i].index == index) break;
	if (i != chipmuck_bp_num) {
		for (env = first_cpu; env != NULL; env = env->next_cpu)
			cpu_breakpoint_remove(env, chipmuck_bps[i].addr);
		chipmuck_bps[i] = chipmuck_bps[chipmuck_bp_num - 1];
		chipmuck_bp_num--;
		return 0;
	}
	for (i = 0; i < chipmuck_wp_num; i++)
		if (chipmuck_wps[i].index == index) break;
	if (i != chipmuck_wp_num) {
		for (env = first_cpu; env != NULL; env = env->next_cpu)
			cpu_watchpoint_remove(env, chipmuck_wps[i].addr);
		chipmuck_wps[i] = chipmuck_wps[chipmuck_wp_num - 1];
		chipmuck_wp_num--;
		return 0;
	}
	return -E_INVALID_PARAM;
}

int chipmuck_set_breakpoint(int type, void *data, int size, int *index) {
	int i;
	CPUState *env;
	
	switch (type) {
	case EVENT_ID_BREAKPOINT:
		if (size != sizeof(uint32_t))
			return -E_INVALID_PARAM;
		if (chipmuck_bp_num == MAX_BP_NUM)
			return -E_BREAKPOINT_LIST_FULL;
		for (i = 0; i < chipmuck_bp_num; i++)
			if (*(uint32_t *)data == chipmuck_bps[i].addr)
				return -E_BREAKPOINT_DUPLICATED;
	
		chipmuck_bps[chipmuck_bp_num].addr = *(uint32_t *)data;
		chipmuck_bps[chipmuck_bp_num].index = chipmuck_index++;
	
		for (env = first_cpu; env != NULL; env = env->next_cpu)
			if (cpu_breakpoint_insert(env, chipmuck_bps[chipmuck_bp_num].addr) == -1) {
				CPUState *env1;
				for (env1 = first_cpu; env1 != env; env1 = env1->next_cpu)
					cpu_breakpoint_remove(env1, chipmuck_bps[chipmuck_bp_num].addr);
				return -E_INTERNAL_ERROR;
			}
		chipmuck_bp_num++;
		return 0;
	case EVENT_ID_WATCHPOINT:
		if (size != sizeof(uint32_t))
			return -E_INVALID_PARAM;
		if (chipmuck_wp_num == MAX_BP_NUM)
			return -E_BREAKPOINT_LIST_FULL;
		for (i = 0; i < chipmuck_wp_num; i++)
			if (*(uint32_t *)data == chipmuck_wps[i].addr)
				return -E_BREAKPOINT_DUPLICATED;
	
		chipmuck_wps[chipmuck_wp_num].addr = *(uint32_t *)data;
		chipmuck_wps[chipmuck_wp_num].index = chipmuck_index++;
	
		for (env = first_cpu; env != NULL; env = env->next_cpu)
			if (cpu_watchpoint_insert(env, chipmuck_wps[chipmuck_wp_num].addr) == -1) {
				CPUState *env1;
				for (env1 = first_cpu; env1 != env; env1 = env1->next_cpu)
					cpu_watchpoint_remove(env1, chipmuck_wps[chipmuck_wp_num].addr);
				return -E_INTERNAL_ERROR;
			}
		chipmuck_wp_num++;
		return 0;
	}
	return -E_INVALID_PARAM;
}

int chipmuck_dump_data(unsigned int addr, void *data, int size) {
	if (cpu_memory_rw_debug(chipmuck_env, addr, (uint8_t *)data, size, 0) == -1)
		return -E_INTERNAL_ERROR;
	return 0;
}

int chipmuck_get_reg(unsigned int *regs, int *nreg) {
	int i;
	
	if (chipmuck_env == 0)
		return -E_INTERNAL_ERROR;
	for (i = 0; i < 8; i++)
		regs[i] = chipmuck_env->regs[i];
	regs[8] = chipmuck_env->eip;
	regs[9] = chipmuck_env->eflags;
	*nreg = 10;
	return 0;
}
